classdef nlCYCLE < nlSLIP
    %UNTITLED Summary of this class goes here
    %   Detailed explanation goes here
    
    properties
        cycle;
        optim;
    end
    
    methods
        
        function res = find_walking_gaits(obj)
           y0=0.9:-0.01:0.8;   %11
           v=0.8:0.01:0.9;      %11
           phi=-0.3:0.01:0.3;   %61
           for idx=1:length(y0)
               for jdx=1:length(v)
                   for kdx=1:length(phi)
                       A=nlCYCLE;
                       A.state.IC=[0 v(jdx)*cos(phi(kdx)) y0(idx) v(jdx)*sin(phi(kdx))];
                       A.state.xe=A.state.IC;
                       [aTD,X,exitflag]=fminbnd(@(aTD) walk_cycle_optim(A,aTD),deg2rad(55),deg2rad(85));
                       if exitflag == 1 && X<1
                           res(kdx,:)=[A.state.IC aTD X];
                       end
                   end
               end
           end         
        end
        
        function obj = walk_cycle(obj)
            obj.cycle.phase=1;
            obj = int_walk_single(obj);
            if obj.intprops.ie==1
                obj.cycle.phase=2;
                obj = int_walk_double(obj);
                if obj.intprops.ie==1
                    obj.intprops.vlo=-1;
                    obj = int_walk_single(obj);
                end
            end
        end
        
        function delta = walk_cycle_optim(obj,aTD)
            clf
            hold on
            xlim([0 1]);
            ylim([0 1]);
            obj.params.aTD=aTD;
            obj.cycle.phase=1;
            obj = int_walk_single(obj);
            %patch([obj.state.x(1,1) obj.state.x(end,1) obj.state.x(end,1) obj.state.x(1,1)],[obj.state.x(1,3) obj.state.x(1,3) obj.state.x(end,3) obj.state.x(end,3)],[0.7 0.7 0.7])
            p=size(obj.state.x,1);
            plot(obj.state.x(:,1),obj.state.x(:,3),'r')
            text(0,0.2,['aTD: ',num2str(rad2deg(obj.params.aTD))]);
            drawnow
            if obj.intprops.ie==1
                obj.cycle.phase=2;
                obj = int_walk_double(obj);
                plot(obj.state.x(p:end,1),obj.state.x(p:end,3),'g')
                p=size(obj.state.x,1);
                plot(obj.state.FP(:,1),obj.state.FP(:,2),'k*')
                %patch([obj.state.x(p+1,1) obj.state.x(end,1) obj.state.x(end,1) obj.state.x(p+1,1)],[obj.state.x(p+1,3) obj.state.x(p+1,3) obj.state.x(end,3) obj.state.x(end,3)],[0.4 0.4 0.4])
                drawnow
                if obj.intprops.ie==1
                    obj.intprops.vlo=-1;
                    obj = int_walk_single(obj);
                    plot(obj.state.x(p:end,1),obj.state.x(p:end,3));
                    drawnow
                    if obj.intprops.ie==2
                        delta=sum(abs(obj.state.xe-obj.state.IC));
                    else
                        delta=10;
                    end
                else
                    delta=100;
                end
            else
               delta=1000; 
            end
        end
        
        function obj=matpool(obj)
            try
                a=parcluster;
                obj.optim.mpool=a.NumWorkers;
            catch
                try
                    schd = findResource('scheduler', ...
                        'configuration', defaultParallelConfig);
                    obj.optim.mpool=schd.ClusterSize;
                catch
                    disp('Unable to identify cluster size');
                    obj.optim.mpool=1;
                end
            end
        end
        
    end
    
end

